package algo;

/**
 * @author Ajie
 * @date 8/17/24
 * @function
 */
public class KalmanFilter {
    private double processNoise = 0.00001;;  // 过程噪声协方差
//    private double measurementNoise = 0.01;;  // 测量噪声协方差
    private double measurementNoise = 0.1;;  // 测量噪声协方差
    public double estimatedValue;  // 估计值
    private double errorCovariance = 1.0;  // 误差协方差
//    private double errorCovariance = 0;  // 误差协方差

    public KalmanFilter() {}

    public KalmanFilter(double initialValue) {
        this.estimatedValue = initialValue;
    }

    public KalmanFilter(double processNoise, double measurementNoise, double initialValue) {
        this.processNoise = processNoise;
        this.measurementNoise = measurementNoise;
        this.estimatedValue = initialValue;
        this.errorCovariance = 1.0;
    }

    public double update(double measurement) {
        // 预测误差协方差
        errorCovariance += processNoise;

        // 计算卡尔曼增益
        double kalmanGain = errorCovariance / (errorCovariance + measurementNoise);

        // 更新估计值
        estimatedValue += kalmanGain * (measurement - estimatedValue);

        // 更新误差协方差
        errorCovariance = (1 - kalmanGain) * errorCovariance;

        return estimatedValue;
    }
}

//    // 使用示例
//    KalmanFilter kf = new KalmanFilter(initialRssi);
//    double smoothedRssi = kf.update(currentRssi);